#include "mmu.h"
#include "mm.h"


static void alloc_init_pte(pmd_t* pmdp, u64 addr, u64 end, u64 phys, u64 prot) {
	pmd_t pmd = *pmdp;
	pte_t* ptep;

	if (pmd_none(pmd)) {
		u64 pte_phys;
		pte_phys = (u64)alloc_one_page();
		set_pmd(pmdp, __pmd(pte_phys | PMD_TYPE_TABLE));
		pmd = *pmdp;
	}

	ptep = pte_offset_phys(pmdp, addr);
	do {
		set_pte(ptep, pfn_pte(phys >> PAGE_SHIFT, prot));
		phys += PAGE_SIZE;
	} while (ptep++, addr += PAGE_SIZE, addr != end);
}


static void alloc_init_pmd(pud_t* pudp, u64 addr, u64 end, u64 phys, u64 prot) {
	pud_t pud = *pudp;
	pmd_t* pmdp;
	u64 next;

	if (pud_none(pud)) {
		u64 pmd_phys;
		pmd_phys = (u64)alloc_one_page();
		set_pud(pudp, __pud(pmd_phys | PUD_TYPE_TABLE));
		pud = *pudp;
	}

	pmdp = pmd_offset_phys(pudp, addr);
	do {
		next = pmd_addr_end(addr, end);
		alloc_init_pte(pmdp, addr, next, phys, prot);
		phys += next - addr;
	} while (pmdp++, addr = next, addr != end);
}


static void alloc_init_pud(pgd_t* pgdp, u64 addr, u64 end, u64 phys, u64 prot) {
	pgd_t pgd = *pgdp;
	pud_t* pudp;
	u64 next;

	if (pgd_none(pgd)) {
		u64 pud_phys;
		pud_phys = (u64)alloc_one_page();
		set_pgd(pgdp, __pgd(pud_phys | PGD_TYPE_TABLE));
		pgd = *pgdp;
	}

	pudp = pud_offset_phys(pgdp, addr);
	do {
		next = pud_addr_end(addr, end);
		alloc_init_pmd(pudp, addr, next, phys, prot);
		phys += next - addr;
	} while (pudp++, addr = next, addr != end);
}


void __create_pgd_mapping(pgd_t* pgdir, u64 paddr, u64 vaddr, u64 size, u64 prot) {
	pgd_t* pgdp = pgd_offset_raw(pgdir, vaddr);
	u64 addr, end, next;

	paddr &= PAGE_MASK;
	addr   = vaddr & PAGE_MASK;
	end    = (u64)PAGE_ALIGN(vaddr + size);

	do {
		next = pgd_addr_end(addr, end);
		alloc_init_pud(pgdp, addr, next, paddr, prot);
	} while (pgdp++, addr = next, addr != end);
}

